/////////////////////////////////////////////////////////////////////////////
// Filename: Matrix4.cpp
// Description: 4x4 matrix for homogeneous transformations
/////////////////////////////////////////////////////////////////////////////

#include "Matrix4.h"

Matrix4::Matrix4(const vector4 &a,const vector4 &b,
const vector4 &c, const vector4 &d) {
  for (int i=0;i<4;i++) {
    (*this)(0,i)=a[i];
    (*this)(1,i)=b[i];
    (*this)(2,i)=c[i];
    (*this)(3,i)=d[i];
  }
}

Matrix4 Matrix4::operator *(const Matrix4 &T) const {
  Matrix4 R;
  for (int i=0;i<4;i++) {
    for(int j=0;j<4;j++) {
      R(i,j)=0.0;
      for (int k=0;k<4;k++)         
        R(i,j)+=(*this)(i,k)*T(k,j);
    }
  }
  return R;
}

Matrix4 Matrix4::operator *(float f) const {
  Matrix4 T;
  for (int i=0;i<16;i++){
	  T.m[i]=m[i]*f;
  }
  return T;
}

Matrix4 &Matrix4::operator *=(float f) {
  for (int i=0;i<16;i++){
	  m[i]*=f;
  }
  return *this;
}

vector4 Matrix4::operator *(const vector4 &p) const {
  vector4 t;
  for (int i=0;i<4;i++) {
    t[i]=0.0;   
    for(int j=0;j<4;j++) 
      t[i]+=(*this)(i,j)*p[j];
  }
  return t;
}

Matrix4 Matrix4::transpose() const {
  Matrix4 tmp;
  for (int i=0;i<4;i++) {
    for(int j=0;j<4;j++) 
      tmp(i,j)=(*this)(j,i);       
  }
  return tmp;
}


/////////////////////////////////////////////////////////////////////////////
//Obs! det() and inv() works ONLY if the last line of the matrix is (0,0,0,1)
/////////////////////////////////////////////////////////////////////////////

double Matrix4::det() const
{
  return ( (*this)(0,0)*(*this)(1,1)*(*this)(2,2) + (*this)(1,0)*(*this)(2,1)*(*this)(0,2) + (*this)(2,0)*(*this)(0,1)*(*this)(1,2)
  -(*this)(0,0)*(*this)(2,1)*(*this)(1,2) - (*this)(1,0)*(*this)(0,1)*(*this)(2,2) - (*this)(2,0)*(*this)(1,1)*(*this)(0,2));
}

Matrix4 Matrix4::inv() const
{
  float t=float( 1.0f/det() );
  
  Matrix4 tmp;
  
  tmp(0,0)= t*( (*this)(1,1)*(*this)(2,2) - (*this)(1,2)*(*this)(2,1) );
  tmp(0,1)=-t*( (*this)(0,1)*(*this)(2,2) - (*this)(0,2)*(*this)(2,1) );
  tmp(0,2)= t*( (*this)(0,1)*(*this)(1,2) - (*this)(0,2)*(*this)(1,1) );
  tmp(0,3)=-t*((*this)(0,1)*(*this)(1,2)*(*this)(2,3)-(*this)(0,1)*(*this)(1,3)*(*this)(2,2)-(*this)(1,1)*(*this)(0,2)*(*this)(2,3)+(*this)(1,1)*(*this)(0,3)*(*this)(2,2)+(*this)(2,1)*(*this)(0,2)*(*this)(1,3)-(*this)(2,1)*(*this)(0,3)*(*this)(1,2));
  
  tmp(1,0)=-t*( (*this)(1,0)*(*this)(2,2) - (*this)(1,2)*(*this)(2,0) );
  tmp(1,1)= t*( (*this)(0,0)*(*this)(2,2) - (*this)(0,2)*(*this)(2,0) );
  tmp(1,2)=-t*( (*this)(0,0)*(*this)(1,2) - (*this)(0,2)*(*this)(1,0) );
  tmp(1,3)= t*((*this)(0,0)*(*this)(1,2)*(*this)(2,3)-(*this)(0,0)*(*this)(1,3)*(*this)(2,2)-(*this)(1,0)*(*this)(0,2)*(*this)(2,3)+(*this)(1,0)*(*this)(0,3)*(*this)(2,2)+(*this)(2,0)*(*this)(0,2)*(*this)(1,3)-(*this)(2,0)*(*this)(0,3)*(*this)(1,2));
  
  tmp(2,0)= t*( (*this)(1,0)*(*this)(2,1) - (*this)(1,1)*(*this)(2,0) );
  tmp(2,1)=-t*( (*this)(0,0)*(*this)(2,1) - (*this)(0,1)*(*this)(2,0) );
  tmp(2,2)= t*( (*this)(0,0)*(*this)(1,1) - (*this)(0,1)*(*this)(1,0) );
  tmp(2,3)=-t*((*this)(0,0)*(*this)(1,1)*(*this)(2,3)-(*this)(0,0)*(*this)(1,3)*(*this)(2,1)-(*this)(1,0)*(*this)(0,1)*(*this)(2,3)+(*this)(1,0)*(*this)(0,3)*(*this)(2,1)+(*this)(2,0)*(*this)(0,1)*(*this)(1,3)-(*this)(2,0)*(*this)(0,3)*(*this)(1,1));
  
  tmp(3,0)=0; 
  tmp(3,1)=0;
  tmp(3,2)=0; 
  tmp(3,3)=1; 
  
  return tmp;
}

/////////////////////////////////////////////////////////////////////////////
//                               END OF FILE                               //
/////////////////////////////////////////////////////////////////////////////

